/**
 * @file gyros.c
 *
 * @date Sep 29, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
#include <stdbool.h>
#include <stddef.h>

#include <ebex_math.h>
#include <ebex_quaternion.h>
#include <ebex_matrix.h>
#include <log_util.h>

#include <gyros.h>

static void ebex_gyro_noise(gyrobox_t *m_gyros, ebex_3vec_t *m_result, ebex_3vec_t *m_ratenoise, ebex_3vec_t *m_obs)
{
	ebex_mat3x3_t nonidealities;
	ebex_3vec_t noise;

	if (!m_ratenoise)
		for (int i = 0; i < 3; i++) noise.element[i] = ran_sample(&m_gyros->gyro[i].param.sample.rate_noise);
	else
		ebex_3vec_copy(&noise, m_ratenoise);

	for (int i = 0; i < 3; i++)
	{
		if (signbit(m_obs->element[i]))
		{
			ebex_3mat_set_elem(&nonidealities, i, i,
					1.0 - m_gyros->gyro[i].param.value.sym_scaling - m_gyros->gyro[i].param.value.asym_scaling);
		}
		else
		{
			ebex_3mat_set_elem(&nonidealities, i, i,
					1.0 - m_gyros->gyro[i].param.value.sym_scaling + m_gyros->gyro[i].param.value.asym_scaling);
		}
	}

	ebex_3mat_set_elem(&nonidealities, 1, 0, m_gyros->gyro[2].param.value.misalignment);
	ebex_3mat_set_elem(&nonidealities, 2, 0, m_gyros->gyro[1].param.value.misalignment);
	ebex_3mat_set_elem(&nonidealities, 2, 1, m_gyros->gyro[0].param.value.misalignment);
	ebex_3mat_set_elem(&nonidealities, 0, 1, 0.0);
	ebex_3mat_set_elem(&nonidealities, 0, 2, 0.0);
	ebex_3mat_set_elem(&nonidealities, 1, 2, 0.0);

	ebex_3mat_mul_v3(m_result, &nonidealities, &noise);
	ebex_quat_rotate_3vec(m_result, &m_gyros->alignment, m_result);
}

static void ebex_gyro_nonideals(gyrobox_t *m_gyros, ebex_3vec_t *m_result, ebex_3vec_t *m_obs)
{
	double phi2 = -m_gyros->gyro[2].param.value.misalignment;
	double theta1 = m_gyros->gyro[0].param.value.misalignment + M_PI_2;
	double theta2 = m_gyros->gyro[1].param.value.misalignment + M_PI_2;
	double cosphi2 = cos(phi2);
	double sintheta1 = sin(theta1);
	double tantheta1 = tan(theta1);
	double tanphi2 = tan(phi2);

	ebex_3vec_set_x(m_result,
			  m_obs->x * 1.0 / (sin(theta2)*cosphi2)
			- m_obs->y * (tanphi2/sintheta1)
			+ m_obs->z * ((tanphi2/tantheta1) - 1.0 /(tan(theta2)*cosphi2))
			- m_obs->x * m_gyros->gyro[0].param.value.sym_scaling
			+ fabs(m_obs->x) * m_gyros->gyro[0].param.value.asym_scaling);
	ebex_3vec_set_y(m_result,
			  m_obs->y * 1.0 / sintheta1
			- m_obs->z * 1.0 / tantheta1
			- m_obs->y * m_gyros->gyro[1].param.value.sym_scaling
			+ fabs(m_obs->y) * m_gyros->gyro[1].param.value.asym_scaling);
	ebex_3vec_set_z(m_result,
			  m_obs->z
			- m_obs->z * m_gyros->gyro[2].param.value.sym_scaling
			+ fabs(m_obs->z) * m_gyros->gyro[2].param.value.asym_scaling);

//	ebex_quat_rotate_3vec(m_result, &m_gyros->alignment, m_result); /// Misalignment angles are to the pre-rotated frame
}

void ebex_gyro_step(ebex_quat_t *m_result, ebex_quat_t *m_origin, ebex_3vec_t *m_obs, ebex_3vec_t *m_ratenoise, gyrobox_t *m_gyros, double m_sec, bool m_usenoise)
{
	ebex_3vec_t noise;
	ebex_3vec_t nonideals;
	ebex_3vec_t fullgyro;
	ebex_3vec_t observation;
	ebex_3vec_t *precomputed_noise = m_usenoise ? m_ratenoise : NULL;

	/**
	 * This is not actually correct as gyro0,gyro1,gyro2 do not map to observation x,y,z but we make the assumption here
	 * that all three gyros are the same and keep the same saturation value.  If this changes, we will need to re-work
	 * the check here
	 *
	 */
	if (fabs(m_obs->x) > m_gyros->gyro[0].param.value.saturation ||
		fabs(m_obs->y) > m_gyros->gyro[1].param.value.saturation ||
		fabs(m_obs->z) > m_gyros->gyro[2].param.value.saturation) die("Invalid speed!");

	ebex_gyro_noise(m_gyros, &noise, precomputed_noise, m_obs);

	for (size_t i = 0; i < 3; i++)
	{
		observation.element[i] = m_obs->element[i] - m_gyros->gyro[i].param.value.bias;
	}
	ebex_quat_rotate_3vec(&fullgyro, &m_gyros->alignment, &observation);
	ebex_gyro_nonideals(m_gyros, &fullgyro, &fullgyro);
	ebex_3vec_sub(&fullgyro, &fullgyro, &noise);

	ebex_quat_propagate(m_result, m_origin, &fullgyro, m_sec);
}

void ebex_gyro_step_backward(ebex_quat_t *m_result, ebex_quat_t *m_origin, ebex_3vec_t *m_obs, ebex_3vec_t *m_ratenoise, gyrobox_t *m_gyros, double m_sec, bool m_usenoise)
{
	ebex_3vec_t noise;
	ebex_3vec_t nonideals;
	ebex_3vec_t fullgyro;
	ebex_3vec_t observation;
	ebex_3vec_t *precomputed_noise = m_usenoise ? m_ratenoise : NULL;

	/**
	 * This is not actually correct as gyro0,gyro1,gyro2 do not map to observation x,y,z but we make the assumption here
	 * that all three gyros are the same and keep the same saturation value.  If this changes, we will need to re-work
	 * the check here
	 *
	 */
	if (fabs(m_obs->x) > m_gyros->gyro[0].param.value.saturation ||
		fabs(m_obs->y) > m_gyros->gyro[1].param.value.saturation ||
		fabs(m_obs->z) > m_gyros->gyro[2].param.value.saturation) die("Invalid speed!");
	ebex_3vec_neg(&observation, m_obs);
	ebex_gyro_noise(m_gyros, &noise, precomputed_noise, &observation);
//	ebex_gyro_nonideals(m_gyros, &nonideals, &observation);

	for (size_t i = 0; i < 3; i++)
	{
		observation.element[i] += m_gyros->gyro[i].param.value.bias;
	}
	ebex_quat_rotate_3vec(&fullgyro, &m_gyros->alignment, &observation);
	ebex_gyro_nonideals(m_gyros, &fullgyro, &fullgyro);
//	ebex_3vec_add(&fullgyro, &fullgyro, &nonideals);
	ebex_3vec_sub(&fullgyro, &fullgyro, &noise);

	ebex_quat_propagate(m_result, m_origin, &fullgyro, m_sec);
}
